#ifndef __COSTMAP_H
#define __COSTMAP_H
#include <vector>
#include <stdlib.h>

class Costmap
{
private:
    unsigned char *map_;
    unsigned int height_;
    unsigned int width_;
    double resolution_;

public:
    double get_resolution() {return resolution_;}
    unsigned int get_height() {return height_;}
    unsigned int get_width() {return width_;}
    unsigned char* get_char_map() { return map_; }

    unsigned char get_cost_value(unsigned int x,unsigned int y);
    bool world2map(double wx, double wy, unsigned int &mx, unsigned int &my);
    void map2world(unsigned int mx, unsigned int my, double &wx, double &wy);
    void init_map(unsigned int h,unsigned int w,double r);

};

#endif


